/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author Wendy Chan
 */
public class Shooter extends PIDSubsystem {
    private static final double Kp = 0.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;

    public static final double SHOOTSPEED = 100.0; //bogus number

    private SpeedController motor1;
    private SpeedController motor2;
    private DigitalInput hall;
    private Timer sampletime;

    double rate;

    // Initialize your subsystem here
    public Shooter() {
        super("Shooter", Kp, Ki, Kd);
        motor1 = new Jaguar(RobotMap.flywheelMotor1);
        motor2 = new Jaguar(RobotMap.flywheelMotor2);
        hall = new DigitalInput(RobotMap.hallsensor);

        // Use these to get going:
        setSetpoint(SHOOTSPEED); //-  Sets where the PID controller should move the system to
        enable(); // - Enables the PID controller.
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        hall.get();

        return rate;
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        motor1.set(output);
        motor2.set(output);
    }
}